#!/usr/bin/env python3
import Angle
import serial
import time
import threading

class Robot:
    def __init__(self, port='/dev/ttyS7', baudrate=115200,timeout = 0.1):
        """
        初始化机器人控制器
        """
        self.lock=threading.Lock()
        self.ser = serial.Serial(port=port, baudrate=baudrate,timeout = timeout)

        # self.ser.write(bytearray([0x01,0x10,0x00,0xE0,0x00,0x02,0x04,0x00,0x00,0x4E,0x20,0xC9,0x9F]))
        # time.sleep(0.05)
        # self.ser.write(bytearray([0x06,0x10,0x00,0xE0,0x00,0x02,0x04,0x00,0x00,0x4E,0x20,0xD3,0xEB]))
        # time.sleep(0.05)
        # self.ser.write(bytearray([0x01,0x10,0x00,0xE2,0x00,0x02,0x04,0x00,0x00,0x4E,0x20,0x48,0x46]))
        # time.sleep(0.05)
        # self.ser.write(bytearray([0x06,0x10,0x00,0xE2,0x00,0x02,0x04,0x00,0x00,0x4E,0x20,0x52,0x32]))
        # time.sleep(0.05)
        self.joint2_angle=0
        self.joint3_angle=0
        self.joint4_angle=0
        self.joint5_angle=0

        self.motor_commands = {
            # 电机使能指令
            'enable': {
                'motor1': bytearray([0x01,0x06,0x00,0x3e,0x00,0x0f,0xa8,0x02]),
                'motor2': bytearray([0x06,0x06,0x00,0x3e,0x00,0x0f,0xa9,0xb5]),
                'joint1': bytearray([0x02,0x06,0x00,0x3e,0x00,0x0f,0xa8,0x31]),
                'joint2': bytearray([0x03,0x06,0x00,0x3e,0x00,0x0f,0xa9,0xe0]),
                'joint3': bytearray([0x04,0x06,0x00,0x3e,0x00,0x0f,0xa8,0x57]),
                'joint4': bytearray([0x05,0x06,0x00,0x3e,0x00,0x0f,0xa9,0x86])
            },
            # 电机禁用指令
            'disable': {
                'joint1': bytearray([0x02,0x06,0x00,0x3e,0x00,0x06,0x68,0x37]),
                'joint2': bytearray([0x03,0x06,0x00,0x3e,0x00,0x06,0x69,0xe6]),
                'joint3': bytearray([0x04,0x06,0x00,0x3e,0x00,0x06,0x68,0x51]),
                'joint4': bytearray([0x05,0x06,0x00,0x3e,0x00,0x06,0x69,0x80])
                # 其他电机禁用指令...
            },
            #模式切换指令  位置模式
            'position_mode':{
                'joint2': bytearray([0x02,0x06,0x00,0x3c,0x00,0x01,0x88,0x35]),
                'joint3': bytearray([0x03,0x06,0x00,0x3c,0x00,0x01,0x89,0xe4]),
                'joint4': bytearray([0x04,0x06,0x00,0x3c,0x00,0x01,0x88,0x53]),
                'joint5': bytearray([0x05,0x06,0x00,0x3c,0x00,0x01,0x89,0x82])
            },
            #速度模式
            'velocity_mode':{
                'joint2': bytearray([0x02,0x06,0x00,0x3c,0x00,0x03,0x09,0xf4]),
                'joint3': bytearray([0x03,0x06,0x00,0x3c,0x00,0x03,0x08,0x25]),
                'joint4': bytearray([0x04,0x06,0x00,0x3c,0x00,0x03,0x09,0x92]),
                'joint5': bytearray([0x05,0x06,0x00,0x3c,0x00,0x03,0x08,0x43])
            }
        }

        #self.enable_all()

    #获取num号关节当前角度
    # def get_angle_data(self,num):
    #     #2：左负右正
    #     #5：左正右负
    #     #3：上正下负
    #     #4：上正下负
    #     if num==2:
    #         command = bytearray([0x02,0x03,0x00,0x2c,0x00,0x02,0x05,0xF1])
    #     elif num==3:
    #         command = bytearray([0x03,0x03,0x00,0x2c,0x00,0x02,0x04,0x20])
    #     elif num==4:
    #         command = bytearray([0x04,0x03,0x00,0x2c,0x00,0x02,0x05,0x97])
    #     elif num==5:
    #         command = bytearray([0x05,0x03,0x00,0x2c,0x00,0x02,0x04,0x46])

    #     # command = Angle.SendOrder(num)  
    #     print(f"发送命令: {list(command)}")
    #     self.ser.write(command)
    #     print("发送成功")
        
    #     time.sleep(0.5)
    #     print("等待返回值")
    #     response=self.ser.readline()#读取一行

    #     #长度
    #     print(f"返回值: {len(response)}")

    #     if not response or len(response) < 9:
    #         print("无返回值")
    #         return None
    #     return Angle.OrderToAngle(response)
    def get_angle_data(self,num):
        #2：左负右正
        #5：左正右负
        #3：上正下负
        #4：上正下负
        command = Angle.SendOrder(num)  
        #print(f"发送命令: {command}")

        self.ser.reset_output_buffer()#把缓存区清空

        self.send_command(command=command)

        #print("指令发送完毕")
        response=self.ser.readline()#读取一行
        #response=self.ser.read(9)
        #print(f"response lenght:{len(response)}")
        if not response or len(response) != 9:
            #print("无返回值or返回值长度不为9")
            return None
        return Angle.OrderToAngle(response)
      
    def send_command(self, command):
        with self.lock:
            self.ser.write(command)
            time.sleep(0.05)
        
    def Position_mode(self):
        for motor in self.motor_commands['position_mode']:
            self.send_command(self.motor_commands['position_mode'][motor])
            time.sleep(0.1)

    def Velocity_mode(self):
        for motor in self.motor_commands['velocity_mode']:
            self.send_command(self.motor_commands['velocity_mode'][motor])
            time.sleep(0.1)

    def enable_motor(self, motor_name):
        if motor_name in self.motor_commands['enable']:
            self.send_command(self.motor_commands['enable'][motor_name])
        else:
            print(f"未知电机: {motor_name}")
    
    def disable_motor(self, motor_name):
        if motor_name in self.motor_commands['disable']:
            self.send_command(self.motor_commands['disable'][motor_name])
        else:
            print(f"未知电机: {motor_name}")
    
    def enable_all(self):
        """使能所有电机"""
        for motor in self.motor_commands['enable']:
            self.enable_motor(motor)
            time.sleep(0.1)
    
    def disable_all(self):
        """禁用所有电机"""
        for motor in self.motor_commands['disable']:
            self.disable_motor(motor)
            time.sleep(0.1)
    
    #前后履带相同速度前进
    def move_front(self, motor_name, speed):
        self.send_command(Angle.SpeedToOrder(1,-speed))
        self.send_command(Angle.SpeedToOrder(6,speed))
    
    #前后履带相同速度后退   没用上
    # def move_back(self,motor_name,speed):
    #     self.send_command(Angle.SpeedToOrder(1,speed))
    #     self.send_command(Angle.SpeedToOrder(6,speed))

    def stop_move(self):
        self.send_command(Angle.SpeedToOrder(1,0))
        self.send_command(Angle.SpeedToOrder(6,0))


    #2~5号关节电机转动angle度数
    def move_joint_angle(self, joint_name, angle):
        print(f"{joint_name}号电机转动到: {angle}")
        self.send_command(Angle.AngleToOrder(joint_name,angle))

    #拉直，四个关节都回到0°（开机时的状态）
    def move_straight(self):
        for i in range(4):
            self.send_command(Angle.AngleToOrder(i+2,1))
            print(f"joint{i+2} moving to Zero")
            time.sleep(0.5)
    def move_straight1(self): 
        for i in range(4):
            self.ser.reset_input_buffer()
            if i==2:
                self.send_command(Angle.AngleToOrder(i+2,-1))
            else:
                self.send_command(Angle.AngleToOrder(i+2,1))
            response=bytearray()
            while len(response)<8:
                if self.ser.in_waiting>0:
                    response+=self.ser.read(self.ser.in_waiting)
            print(response.hex())
    def move_straight2(self):
        self.ser.reset_input_buffer()
        self.send_command(Angle.AngleToOrder(2,1))
        response = bytearray()
        while len(response) < 8:
            if self.ser.in_waiting > 0:
                response += self.ser.read(self.ser.in_waiting)
        print(response.hex())
    #2~5号关节电机转速控制
    def move_joint_speed(self,joint_name,direction):
        #direction
        # 设为1，向前/右
        # 设为-1，向后/左
        # 设为0，停止
        if direction==1:
            self.send_command(Angle.SpeedToOrder(joint_name,1500))
        elif direction==-1:
            self.send_command(Angle.SpeedToOrder(joint_name,-1500))
        else:
            self.send_command(Angle.SpeedToOrder(joint_name,0))


    def close(self):
        self.ser.close()
    
    def __del__(self):
        if hasattr(self, 'ser') and self.ser.is_open:
            self.ser.close()

